#ifndef PERCEPTION_SENSORLIDAR_LIDAR_NOISE_FILTER_H
#define PERCEPTION_SENSORLIDAR_LIDAR_NOISE_FILTER_H

#include <Eigen/Dense>

#include <ros/ros.h>

#include <pcl/point_cloud.h>

#include <opencv2/core/core.hpp>

#include "common/object_types.h"
#include "utils/rigid.h"
#include "utils/timed_queue.h"

namespace avos {
namespace perception {

using ::common::transform::Rigid3;
using ::common::transform::StampedStorage;
using ::common::transform::TimeCache;
using ::common::transform::TimedQueue;

/// Cells stored coordinate make up with current frame pose.
struct CellsWithPose {
  DummyVector<Eigen::Vector3f> cells;
  Rigid3<float> pose;

  CellsWithPose(const DummyVector<Eigen::Vector3f>& in_cells,
                const Rigid3<float>& in_pose)
      : cells(in_cells), pose(in_pose) {}
  CellsWithPose() = default;
  CellsWithPose(const CellsWithPose& rhs) = default;
  CellsWithPose& operator=(const CellsWithPose& rhs) = default;
};

/// A accumulative grid map for correct types of TINY and SHORT_LOW that misclassified to NOISE.
// FIXME: inefficient data structure
struct GridMapBin {
  GridMapBin(float front_range, float aside_range, float in_resolution)
      : resolution_(in_resolution) {
    rows_ = front_range / resolution_;
    cols_ = aside_range / resolution_;
    data_ = DummyVector<DummyVector<uint8_t>>(rows_,
                                              DummyVector<uint8_t>(cols_, 0));
    origin_x_ = 0;
    origin_y_ = cols_ / 2.0;
  }
  /// Reset needed params.
  void reset() {
    data_ = DummyVector<DummyVector<uint8_t>>(rows_,
                                              DummyVector<uint8_t>(cols_, 0));
  }
  /// Set gridmap coordinate value based on Lidar coordinate x and y.
  /**
   * \param[in] x The Lidar coordinate x.
   * \param[in] y The Lidar coordinate y.
   */
  void set(float x, float y) {
    int col = trunc(y / resolution_) + origin_y_;
    int row = trunc(x / resolution_) + origin_x_;
    if (0 <= col && col <= cols_ - 1 && 0 <= row && row <= rows_ - 1) {
      data_[row][col] = 1;
    }
  }

  // TODO: inflation grid map according to move direction
  void inflation(const float move_dir_x, const float move_dir_y) {}

  /// Get gridmap coordinate value based on Lidar coordinate x and y or return 0 if value is NAN.
  /**
   * \param[in] x The Lidar coordinate x.
   * \param[in] y The Lidar coordinate y.
   */
  int get(float x, float y) {
    int col = round(y / resolution_) + origin_y_;
    int row = round(x / resolution_) + origin_x_;
    if (0 <= col && col <= cols_ - 1 && 0 <= row && row <= rows_ - 1) {
      return data_[row][col];
    } else {
      return 0;
    }
  }

  // cv::Mat visulize() {
  //   cv::Mat cv_mat = cv::Mat::zeros(cv::Size(cols_, rows_), CV_8UC3);
  //   for (int col = 0; col < cols_; col++) {
  //     for (int row = 0; row < rows_; row++) {
  //       cv_mat.at<cv::Vec3b>(row, col) = cv::Vec3b(255, 255, 255);
  //     }
  //   }
  //   return cv_mat;
  // }

  /// Visualize gridmap points for debug.
  pcl::PointCloud<pcl::PointXYZ> visualize() {
    pcl::PointCloud<pcl::PointXYZ> grid_cloud;
    for (float y = -5; y < 5; y = y + 0.1) {
      for (float x = 0; x < 7; x = x + 0.1) {
        int col = round(y / resolution_) + origin_y_;
        int row = round(x / resolution_) + origin_x_;
        if (0 <= col && col <= cols_ - 1 && 0 <= row && row <= rows_ - 1) {
          if (data_[row][col] > 0) {
            pcl::PointXYZ pcl_point;
            pcl_point.x = x;
            pcl_point.y = y;
            pcl_point.z = -0.2;
            grid_cloud.points.push_back(pcl_point);
          }
        }
      }
    }
    return grid_cloud;
  }

 private:
  const float resolution_;
  /// Two-dimensional gridmap index of row.
  int rows_;
  /// Two-dimensional gridmap index of col.
  int cols_;
  /// a origin x value for translate coordinate from Lidar to gridmap.
  float origin_x_;
  /// a origin y value for translate coordinate from Lidar to gridmap.
  float origin_y_;
  /// Two-dimensional array that stored gridmap data.
  DummyVector<DummyVector<uint8_t>> data_;
};

/// The noise objects filter class.
/**
 * Maintain a own accumulative grid map 
 * for correct types of TINY and SHORT_LOW that misclassified to NOISE.
 */
class LidarNoiseFilter {
 public:
  LidarNoiseFilter(double stamped_buffer_length, double latency);
  /// Lidar_objects filter to correct misclassified objects.
  /**
   * \param[in] data_time The ros time value.
   * \param[in] lidar_objects Lidar objects in one points frame.
   * \param[in] current_pose Current pose get from localization.
   */
  void Filter(ros::Time data_time, DummyVector<LidarObject>& lidar_objects,
              Rigid3<float> current_pose);
  /// Visualize gridmap points for debug.
  void Visualize(pcl::PointCloud<pcl::PointXYZ>& cells_cloud);

 private:
  const double time_latency_;
  GridMapBin grid_map_;
  TimedQueue<CellsWithPose> cells_with_pose_list_;

  // DEBUG value
  DummyVector<Eigen::Vector3f> accumulate_local_cells_;
};
}  // namespace perception
}  // namespace avos
#endif  // PERCEPTION_SENSORLIDAR_LIDAR_NOISE_FILTER_H